A Nearest Neighbor Method for Efficient ICP

A novel solution is presented to the Nearest Neighbor Problem that is specifically tailored for determining correspondences within the Iterative Closest Point Algorithm. The reference point set P is preprocessed by calculating for each point ~pi 2 P that neighborhood of points which lie within a certain distance of ~pi. The points within each -neighborhood are sorted by increasing distance to their respective ~ pi. At runtime, the correspondences are tracked across iterations, and the previous correspondence is used as an estimate of the current correspondence. If the estimate satisfies a constraint, called the Spherical Constraint, then the nearest neighbor falls within the -neighborhood of the estimate. A novel theorem, the Ordering Theorem, is presented which allows the Triangle Inequality to efficiently prune points from the sorted -neighborhood from further consideration. The method has been implemented and is demonstrated to be more efficient than both the k-d tree and Elias methods. After 40 iterations, fewer than 2 distance calculations were required on average per correspondence, which is close to the theoretical minimum of 1. Furthermore, after 20 iterations the time expense per iteration was demonstrated to be negligibly more than simply looping through the points.